#!/usr/bin/env python
#coding=utf-8
from flask.scaffold import F
from numpy import number
import roslibpy
import signal
import sys

sys.path.insert(0, '/home/center/fdse_coffee/src/jaka_pro/lib')
sys.path.insert(0, '/home/center/fdse_coffee/src/jaka_pro')

import argparse
import time
import socket
import threading
from lib.jaka_and_gripper_twocup import Jaka, Gripper
from tqdm import tqdm
from copy import deepcopy
# from lib.para_utils import *
from flask import Flask, request
import requests

app = Flask(__name__)

class Jarvis(object):
    def __init__(self, control_center_ip='192.168.1.133', jaka_ip='192.168.1.144', port=8234,
                 gripper_ip='192.168.1.29'):
        super(Jarvis, self).__init__()
        self.mode = 'pepper'
        self.control_center_ip = control_center_ip
        self.pepper_ip = '192.168.100.64'
        self.control_center_port = port
        self.gripper_ip = gripper_ip
        self.coffee_num = {'USA': '4', 'ITALY': '5', 'NORMAL': '7'}
        self.order = []
        self.jaka_ip = jaka_ip
        self.dis = -1
        self.test = False
        self.send_msg = '[[[AT CF{}|||COFFEE BUTTON {} PRESSED 100 ms]]]\r\n'
        self.coffee_finished = 0
        self.seat_num_str_list=[]
        self.pepper_isonline=False

        print('control_center_ip:port = {}:{}'.format(self.control_center_ip, self.control_center_port))
        print('jaka_ip = {}'.format(self.jaka_ip))
        print('gripper_ip = {}'.format(self.gripper_ip))

        if not self.test:
            self.ros_mini = roslibpy.Ros('192.168.100.10', 9090)
            self.ros_mini.run()
            if not self.ros_mini.is_connected:
                print("Not connected to ROS mini.")
                sys.exit()

            self.tof_topic = roslibpy.Topic(self.ros_mini, '/nlink_tofsense_frame0', 'nlink_parser/TofsenseFrame0')
            self.tof_topic.subscribe(self.process_tof)
            time.sleep(0.5)

            # rospy.init_node('coffee_king', anonymous=True)
            self.param_init()
        self.robot_state = 0        # if in work
        self.robot = None
        self.order_sys = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.order_sys.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
        self.order_sys.connect((control_center_ip, port))

        self.sock_control = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock_control.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
        self.sock_control.connect((control_center_ip, 8235))
        try:
            self.sock_pepper = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.sock_pepper.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
            self.sock_pepper.connect(('localhost', 9001))
            print('===================pepper is online=============')
            self.pepper_isonline=True
        except:
            print('pepper is not online')
       
        if self.dis == -1:
            flag = 0
            for i in range(3):
                time.sleep(2)
                if self.dis == -1:
                    continue
                else:
                    flag = 1
                    break
            if flag == 0:
                print('Distance module has problem, exit.')
                self.order_sys.close()
                self.sock_control.close()
                sys.exit()
        elif self.dis > 0.1:
            self.drop_cup()
  
        
        self.jaka_init()
        print('jaka init successful')
        

        self.t1 = threading.Thread(target=self.receive_order_center)
        self.t1.setDaemon(True)
        self.t1.start()
       

        self.t2 = threading.Thread(target=self.receive_order_pepper)
        self.t2.setDaemon(True)
        self.t2.start()

        time.sleep(2)
        print('======初始化成功======')

    def drop_cup(self):
        self.sock_control.sendall(self.send_msg.format('08', '8').encode('utf-8'))

    def param_init(self):
        self.ros_mini.set_param('/peanut/coffee_place', 'no')
        self.ros_mini.set_param('/peanut/ready', 'no')
        self.ros_mini.set_param('/order/ready', 'no')
        self.ros_mini.set_param('/order/table_id', '0')
        self.ros_mini.set_param('/peanut/table_id', '-1')

    def process_tof(self, msg):
        system_time = msg['system_time']
        signal_streng = msg['signal_strength']
        dis_status = msg['dis_status']
        self.dis = msg['dis']
        id = msg['id']
        # print('callback', self.dis)

    def get_param(self, ros_master, param):
        return ros_master.get_param(param)

    def set_param(self, ros_master, param, value):
        return ros_master.set_param(param, value)

    def wait_param(self, ros_master, param, value):
        while (1):
            current_value = ros_master.get_param(param)
            # print('Waiting for {}: {} --- Now is {}'.format(param, value, current_value))
            if value == current_value:
                # print("{} has become {}".format(param, value))
                return
            time.sleep(1)

    def jaka_init(self):
        Gripper.init(ip=self.gripper_ip, port=8888)
        time.sleep(1)
        self.robot = Jaka(jaka_ip=self.jaka_ip)
        self.robot_state = 1
        # print('22')
        self.robot.go_home()
        _ = self.robot.get_pose(print_flag=True)
        # print('1')
        time.sleep(1)

    def pause_init(self):
        self.jaka_init()
        self.sock_control = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.sock_control.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
        self.sock_control.connect((self.control_center_ip, 8235))

    def is_work(self):
        return self.robot_state

    def jaka_close(self):
        self.robot.close()
        self.robot_state = 0
        print('机器人已断开连接')

    def pause_close(self):
        self.jaka_close()

        self.sock_control.close()

        self.sock_pepper.close()

    def close(self):
        self.jaka_close()
        self.order_sys.close()
        self.sock_control.close()
        self.sock_pepper.close()
        #if self.mode == 'control':
        #    self.sock_control.close()
        #elif self.mode == 'pepper':
        #    self.sock_pepper.close()

    def receive_order_center(self):
        while True:
            try:
                data = self.order_sys.recv(1024)
                print('receive_order order', data)
            except socket.error as msg:
                print(time.strftime("%m-%d_%H:%M", time.localtime()))
                print('receive_order error', msg)
                self.order_sys.close()
                self.order_sys = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.order_sys.connect((self.control_center_ip, self.control_center_port))
                self.order_sys.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
                print('Control center has reconnect...')
                continue
            ori_data = deepcopy(data)
            re = '[[[AT CF ORDER OK]]]\r\n'
            self.order_sys.sendall(re)
            d = self.order_sys.recv(1024)
            coffee_index = data.index('|||')
            coffee = data[:coffee_index].split(' ')[-1]
            seat_index = data.index(']]]')
            seat = data[:seat_index].split(' ')[-1]
            print('----------------------')
            print('接收到一个来自center的新订单：')
            print('咖啡：{}, 座位号：{}'.format(coffee, seat))
            print('----------------------')
            self.order.append((coffee, int(seat)))

    def receive_order_pepper(self):
        while True:
            if not self.pepper_isonline:
                try:
                    self.sock_pepper = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                    self.sock_pepper.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
                    self.sock_pepper.connect(('localhost', 9001))
                    print('===================pepper is online=============')
                    self.pepper_isonline=True
                except:
                    self.pepper_isonline=False
                    continue
            else:
                pass

            try:
                data = self.sock_pepper.recv(1024)
                print('receive_order pepper', data)
            except socket.error as msg:
                '''
                print(time.strftime("%m-%d_%H:%M", time.localtime()))
                print('receive_order error', msg)
                self.sock_pepper.close()
                self.sock_pepper = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
                self.sock_pepper.connect(('localhost', 9001))
                self.sock_pepper.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1)
                print('Pepper serer has reconnected...')
                '''
                continue
                
            try:
                #ori_data = deepcopy(data)
                re = '[[[AT CF ORDER OK]]]\r\n'
                self.sock_pepper.sendall(re)
            except:
                print('send response failed')
                continue
            #d = self.sock_pepper.recv(1024)
            print('=============================')
            print(data)
            try:
                coffee_index = data.index('|||')
            except:
                continue
            coffee = data[:coffee_index].split(' ')[-1]
            seat_index = data.index(']]]')
            seat = data[:seat_index].split(' ')[-1]
            print('----------------------')
            print('接收到一个来自pepper的新订单：')
            print('咖啡：{}, 座位号：{}'.format(coffee, seat))
            print('----------------------')
            self.order.append((coffee, int(seat)))

    def get_order(self):
        # if self.test:
        #     return '3'
        if len(self.order) != 0:
            print('目前所有订单：')
            for order in self.order:
                print('咖啡：{}, 座位号：{}'.format(order[0], order[1]))
            curr_order = self.order.pop(0)
            coffee = curr_order[0]
            seat = curr_order[1]
            return coffee, seat
        else:
            return False, False

    def create_order(self):
        # if self.test:
        #     return '3'
        return 'USA', 3

    def run(self, coffee_cate, seat_num):
        print('＝＝＝＝＝＝＝＝＝＝＝＝')
        print('开始一个新订单：　　　　')

        flag = 0
        for i in range(5):
            print('dis', self.dis)
            if self.dis < 0.1:
                flag = 1
                break
            else:
                time.sleep(0.1)
        if not flag:
            print('落杯器发生故障，退出!')
            self.close()
            sys.exit()



        if not self.test:
            print('咖啡：{}, 座位号{}'.format(coffee_cate, seat_num))
            self.seat_num_str_list.append(str(seat_num))
            if len(self.seat_num_str_list) >=2:
                self.seat_num_str = '_'.join(self.seat_num_str_list)
            else:
                self.seat_num_str = self.seat_num_str_list[0]

            print('==========='+ self.seat_num_str +'=============')
            self.ros_mini.set_param('/order/table_id', self.seat_num_str)
            self.ros_mini.set_param('/order/ready', 'yes')


        print('全力运输中．．．')
        self.robot.catch_and_place(test=self.test)
        self.drop_cup()
        if not self.test:
            coffee_num = self.coffee_num[coffee_cate]
            order = '[[[AT CF{}|||COFFEE BUTTON {} PRESSED 100 ms]]]\r\n'.format(coffee_num.zfill(2), coffee_num)
            # coffee
            self.sock_control.sendall(order)
            print('\n咖啡制作中，请稍后(*^▽^*)')
            for i in tqdm(range(70)):
                time.sleep(1)
            print('\n咖啡制作完成，等待送餐机器人．．．')
            self.wait_param(self.ros_mini, '/peanut/ready', 'yes')
            time.sleep(5.0)
        print('送餐机器人已就绪，全力运输中．．．')
        self.robot.move_to_car(self.coffee_finished, test=self.test)
        print('摆放第{}杯咖啡'.format(str(self.coffee_finished+1)))
        self.coffee_finished += 1
        if self.coffee_finished == 2:
            self.ros_mini.set_param('/peanut/coffee_place', 'yes')
            self.ros_mini.set_param('/peanut/table_id', self.seat_num_str)
            self.coffee_finished = 0
            self.seat_num_str_list=[]
            print('已经放了2杯了,工作完成,下次见～')
            print('＝＝＝＝＝＝＝＝＝＝＝＝')
        elif self.coffee_finished==1 and len(self.order)==0:
            self.ros_mini.set_param('/peanut/coffee_place', 'yes')
            self.ros_mini.set_param('/peanut/table_id', self.seat_num_str)
            self.coffee_finished = 0
            self.seat_num_str_list=[]
            print('工作完成,下次见～')
            print('＝＝＝＝＝＝＝＝＝＝＝＝')
        '''
        elif len(self.order)>0 and self.coffee_finished >2:
            self.ros_mini.set_param('/peanut/coffee_place', 'yes')
            self.coffee_finished = 0
            self.seat_num_str_list=[]
            print('已经放了2杯了，下一趟再来吧～')
            print('＝＝＝＝＝＝＝＝＝＝＝＝')
        '''

    
    def get_cup(self):
        coffee_cate = self.ros_mini.get_param('/order/coffee_cate')
        seat_num = self.ros_mini.get_param('/order/table_id')
        self.seat_num_str_list.append(str(seat_num))
        self.seat_num_str = self.seat_num_str_list[0]
        self.ros_mini.set_param('/order/table_id', self.seat_num_str)
        self.ros_mini.set_param('/order/ready', 'yes')
        self.robot.catch_and_place(test=self.test)
        self.drop_cup()
        
        
    def make_coffee(self,coffee_type):
        # coffee_cate = self.ros_mini.get_param('/order/coffee_cate')
        coffee_num = self.coffee_num[coffee_type]
        order = '[[[AT CF{}|||COFFEE BUTTON {} PRESSED 100 ms]]]\r\n'.format(coffee_num.zfill(2), coffee_num)
        # coffee
        self.sock_control.sendall(order.encode())
        print('\n咖啡制作中，请稍后(*^▽^*)')
        for i in tqdm(range(70)):
            time.sleep(1)   
        print('\n咖啡制作完成，等待送餐机器人．．．')
        # self.wait_param(self.ros_mini, '/peanut/ready', 'yes')
        time.sleep(5.0)
        # event finish requests

    
    def put_cup(self,coffee_num, flag):
        print("coffee_num:",coffee_num)
        print(type(coffee_num))
        print("flag:",flag)
        print(type(flag))
        coffee_num = int(coffee_num)
        flag = int(flag)
        self.robot.move_to_car(coffee_num, test=self.test)
        if(flag==2):
            self.ros_mini.set_param('/peanut/coffee_place', 'yes')

    def get__order(self):
        coffee_cate = 'USA'
        seat_num = 3
        ros_mini = roslibpy.Ros('192.168.100.10', 9090)
        ros_mini.run()
        ros_mini.set_param('/peanut/coffee_place', 'no')
        ros_mini.set_param('/peanut/ready', 'no')
        ros_mini.set_param('/order/ready', 'no')
        ros_mini.set_param('/order/coffee_cate', coffee_cate)
        ros_mini.set_param('/order/table_id', seat_num)
        ros_mini.set_param('/peanut/table_id', seat_num)

    def put_to_plate(self):
        self.robot.put_to_plate()

    # print u'yes'
control_center = Jarvis(control_center_ip='192.168.1.133',
                             jaka_ip='192.168.1.144')


# control_center.get_cup()
# control_center.put_cup(1,1)
# control_center.get_cup()
# control_center.put_cup(2,2)

@app.route('/')
def hello():
    return 'hello'

@app.route("/get_cup/", methods=["post"])
def get_cup():
    control_center.get_cup()
    return ''

@app.route("/put_cup/", methods=["post"])
def put_cup():
    coffee_num = request.json['num']
    flag = request.json['flag']
    control_center.put_cup(coffee_num,flag)
    # thread peanut wait cup
    return ''

@app.route("/make_coffee/", methods=["post"])
def make_coffee():
    #str = request.values['coffee_event']
    #print(str)
    # print(request.json)
    # coffee_cate = 'USA'
    coffee_cate = request.json['coffee_type']
    print(coffee_cate)
    control_center.make_coffee(coffee_cate)
    #requests.get(str)
    return ''

@app.route("/get_order/", methods=["post"])
def get_order_1():
    control_center.get__order()
    return ''

@app.route("/put_to_plate/", methods=["post"])
def put_to_plate():
    control_center.put_to_plate()
    return ''

if __name__ == '__main__':
    
    # mode, seat = control_center.create_order()
    # if not control_center.is_work():
    #     control_center.pause_close()
    # app.debug = False

    app.run(host="0.0.0.0", port=5001)

